#include <Wire.h>
#include <Comm.h>


#define PIN_ITR_C 3       // pin of collector of ITR 9608
#define PIN_SPEED_UP   12
#define PIN_SPEED_DOWN 11


// frame size without df_head
#define DF_I2C_IN  6    // from driver
#define DF_I2C_OUT 2    // to driver
#define DF_SER_IN  2    // from upper
#define DF_SER_OUT 10    // to upper


#define SLAVE_ADDR 10


typedef enum {
  SPEED_UP, SPEED_DOWN, SPEED_STAY, SPEED_CONFIRM
} SpeedAction;

/* ==========================================
 *  Global Varaiables
 * ========================================== */
byte g_buf_in[10];
byte g_buf_out[10];

float g_speed_tar_ctrl = 0;     // target speed calcultated by controller 
//float g_speed_tar_upper = 0;    // target speed from upper
float g_speed_meas_drv = 0;     // measured speed from driver

DataFrame g_df_i2c_in;
DataFrame g_df_ser_in;


/* ==========================================
 *  Functions
 * ==========================================*/
bool TimeUp() {
  static unsigned long _this_time, _last_time;
  _this_time = millis();
  if (_this_time - _last_time > 200) {
    _last_time = _this_time;
    return true;
  }
  return false;
}


// @param tar_ctrl: use which target speed
void write_i2c(bool tar_ctrl) {
  if (tar_ctrl) {
    S16D1ToBs(&(g_buf_in[0]), g_speed_tar_ctrl);
  }

  Wire.beginTransmission(SLAVE_ADDR);
  Wire.write(DF_HEAD);
  Wire.write(g_buf_in, DF_I2C_OUT);
  Wire.endTransmission();
}


status_t read_i2c() {
  status_t _ret = ERR_UNKNOWN;
  
  // read from driver through i2c
  Wire.requestFrom(SLAVE_ADDR, DF_I2C_IN + 1);
  delay(10);  // wait the data to be received
  while (Wire.available()) {
    int ch = Wire.read();
    _ret = DF_Feed(&g_df_i2c_in, ch);
    if (RECV_DONE == _ret) {break;}
  }
  return _ret;
}


status_t read_ser() {
  status_t _ret = ERR_UNKNOWN;
  
  // read from upper through serial
  if (Serial.available() <= DF_SER_IN + 1) {
    while (Serial.available()) {
      int ch = Serial.read();
      _ret = DF_Feed(&g_df_ser_in, ch);
      if (RECV_DONE == _ret) {break;}
    }
  }
  
  return _ret;
}


/* @brief read target speed from upper and send to driver 
 */
void drv_to_upper() {
  status_t _ret = read_i2c();

  // write the data to upper through serial
  if (RECV_DONE == _ret) {
    // the data that is read from i2c and stay unchanged:
    // g_buf_in[0] s16d1, speed_meas_drv
    // g_buf_in[2] s16d1, speed_tar_drv
    // g_buf_in[4] s16d1, sys_v
    S16D1ToBs(&(g_buf_in[6]), ReadSpeed());
    S16D1ToBs(&(g_buf_in[8]), g_speed_tar_ctrl);
    Serial.write(DF_HEAD);
    Serial.write(g_buf_in, DF_SER_OUT);
  }
}


/* @brief read status from driver and report to upper
 */
void upper_to_drv() {
  status_t _ret = read_ser();

  // write to driver through i2c
  if (RECV_DONE == _ret) {
    write_i2c(false);
  }
}


void setup() {
  pinMode(PIN_ITR_C, INPUT);
  pinMode(PIN_SPEED_UP, INPUT_PULLUP);
  pinMode(PIN_SPEED_DOWN, INPUT_PULLUP);

  attachInterrupt(digitalPinToInterrupt(PIN_ITR_C), Sample, FALLING);
  
  Wire.begin();
  Serial.begin(57600);

  // the two struct does not be used at the same time, 
  // so they can share the same buffer memory.
  DF_Init(&g_df_i2c_in, g_buf_in, DF_I2C_IN, DF_I2C_IN);
  DF_Init(&g_df_ser_in, g_buf_in, DF_SER_IN, DF_SER_IN);
}


void loop() {
  upper_to_drv();
  
  if (TimeUp()) {
    drv_to_upper();
    DoBtnAction(ReadButton());
  }
}
